#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define SDA_PIN 27
#define SCL_PIN 26
TwoWire I2Cone = TwoWire(0);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &I2Cone, OLED_RESET);

// 定义引脚
#define THROTTLE_PIN 34
#define JOYSTICK_X_PIN 35
#define JOYSTICK_Y_PIN 32
#define POTENTIOMETER_PIN 33
#define THROTTLE_SWITCH_PIN 25

// 全局变量
int throttleValue = 0;
int joystickX = 0;
int joystickY = 0;
int potentiometerValue = 0;
int throttleSwitchState = 0;
String joystickDirection = "CENTER";

void setup() {
  Serial.begin(115200);
  
  // 初始化自定义I2C引脚
  I2Cone.begin(SDA_PIN, SCL_PIN, 400000);
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 display allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  
  // 设置输入引脚
  pinMode(THROTTLE_SWITCH_PIN, INPUT_PULLUP);
}

void loop() {
  // 读取传感器值
  readSensors();
  
  // 处理传感器数据
  processSensorData();
  
  // 在OLED上显示数据
  displayData();
  
  delay(100); // 控制刷新频率
}

void readSensors() {
  throttleValue = analogRead(THROTTLE_PIN);
  joystickX = analogRead(JOYSTICK_X_PIN);
  joystickY = analogRead(JOYSTICK_Y_PIN);
  potentiometerValue = analogRead(POTENTIOMETER_PIN);
  throttleSwitchState = digitalRead(THROTTLE_SWITCH_PIN);
}

void processSensorData() {
  // 确定摇杆方向
  joystickDirection = "CENTER";
  if (joystickX < 300) joystickDirection = "LEFT";
  if (joystickX > 3900) joystickDirection = "RIGHT";
  if (joystickY < 300) joystickDirection = "UP";
  if (joystickY > 3900) joystickDirection = "DOWN";
  
  // 将电位器值转换为-100到100的范围
  potentiometerValue = map(potentiometerValue, 0, 4095, -100, 100);
}

// 其他代码保持不变...

void displayData() {
  display.clearDisplay();
  
  // 显示油门开关状态
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("Throttle: ");
  display.println(throttleSwitchState == LOW ? "ON" : "OFF");
  
  // 显示摇杆方向（箭头）
  display.setCursor(0, 12);
  display.print("Joystick: ");
  
  int arrowX = 64;  // 箭头中心点X坐标
  int arrowY = 20;  // 箭头中心点Y坐标
  int arrowSize = 10;  // 箭头大小
  
  display.fillRect(arrowX - 15, arrowY - 15, 30, 30, SSD1306_BLACK); // 清除箭头区域
  
  if (joystickDirection == "UP") {
    // 上箭头
    display.drawLine(arrowX, arrowY - arrowSize, arrowX, arrowY + arrowSize, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY - arrowSize, arrowX - arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY - arrowSize, arrowX + arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "DOWN") {
    // 下箭头
    display.drawLine(arrowX, arrowY - arrowSize, arrowX, arrowY + arrowSize, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY + arrowSize, arrowX - arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX, arrowY + arrowSize, arrowX + arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "LEFT") {
    // 左箭头 - 修正了坐标错误
    display.drawLine(arrowX - arrowSize, arrowY, arrowX + arrowSize, arrowY, SSD1306_WHITE);
    display.drawLine(arrowX - arrowSize, arrowY, arrowX - arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX - arrowSize, arrowY, arrowX - arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else if (joystickDirection == "RIGHT") {
    // 右箭头 - 修正了坐标错误
    display.drawLine(arrowX - arrowSize, arrowY, arrowX + arrowSize, arrowY, SSD1306_WHITE);
    display.drawLine(arrowX + arrowSize, arrowY, arrowX + arrowSize/2, arrowY - arrowSize/2, SSD1306_WHITE);
    display.drawLine(arrowX + arrowSize, arrowY, arrowX + arrowSize/2, arrowY + arrowSize/2, SSD1306_WHITE);
  } else {
    // 中心
    display.drawCircle(arrowX, arrowY, arrowSize/2, SSD1306_WHITE);
  }
  
  // 显示电位器值（能量条+数字）
  display.setCursor(0, 40);
  display.print("Potentiometer: ");
  
  // 绘制能量条背景
  display.drawRect(100, 40, 20, 8, SSD1306_WHITE);
  
  // 计算能量条高度（根据电位器值-100到100）
  int barHeight = map(potentiometerValue, -100, 100, 0, 8);
  
  // 绘制能量条
  display.fillRect(101, 48 - barHeight, 18, barHeight, SSD1306_WHITE);
  
  // 显示电位器数字值
  display.setCursor(70, 40);
  display.print(potentiometerValue);
  
  display.display();
}    
